import math
import sys
import numpy as np
    
class controller():
    def __init__(self):
        print('automatic::controller is running')
        self.controller = None
        self.world = None
        self.speed = 0
        self.oldSpeed = 0
        self.speedAdd = 0
        self.direction = 0

    def getVechicalController(self,controller):
        print('automatic::controller getVechicalController')        
        self.controller = controller
        self._control = controller

    def getWorld(self,world):
        print('automatic::controller getWorld')        
        self.world = world       

    
    def setRun(self,speed = None,direction = None):
        print('automatic::controller setRun speed = %s direction = %s'%(speed,direction))
        if speed is not None:
            self.speed = speed
        if direction is not None:
            self.direction = direction

    def getSpeed(self):
        speed = None
        try:
            v = self.world.vehicle.get_velocity()
            speed = (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2))
        except:
            print("Unexpected error:", sys.exc_info()[0])
            print(sys.exc_info())
        return speed

    def throttleFunction(self,detaSpeed,changeSpeed,addSpeed):
        print('detaSpeed:%0.2f changeSpeed:%0.2f addSpeed:%0.2f'%(detaSpeed,changeSpeed,addSpeed))
        throttle = 0
        if detaSpeed >= 0.03:
            throttle = 0
        elif detaSpeed < 0:
            thr = np.abs(detaSpeed) * 0.2 + changeSpeed * -0.1 + addSpeed * -0.1
            throttle = np.max([np.min([thr,1]),0])

        return throttle



    def tick(self):
        if self.controller == None or self.world == None:
            print('controller is not ready  vechical cantroller:%s world:%s'%(not self.controller == None,not self.world == None))
            return
        
        speed = self.getSpeed()
        detaSpeed = speed - self.oldSpeed
        self.oldSpeed = speed
        self.speedAdd = 1 * self.speedAdd + (speed - self.speed)


        if speed == None:
            return



        if self.speed == 0:
            self._control.brake = 1.0
            self._control.throttle = 0.0
        else:
            self._control.brake = 0.0
            self._control.throttle = self.throttleFunction(speed - self.speed,detaSpeed,self.speedAdd)

        

